#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "cloth.h"
#include "argparser.h"
#include <fstream>
#include "vectors.h"
#include <cmath>


using namespace std;

// ============================================================================
// some helper drawing functions

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);

// ============================================================================
// ============================================================================

Cloth::Cloth(ArgParser *_args) {
  args =_args;

  // open the file
  ifstream istr(args->cloth_file.c_str());
  assert (istr != NULL);
  string token;

  // read in the simulation parameters
  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_structural; assert (token == "k_structural");  
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");
  // NOTE: correction factor == .1, means springs shouldn't stretch more 
  //    than 10%, correction factor == 100, means don't do any correction
  istr >> token >> provot_structural_correction; 
  assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; 
  assert (token == "provot_shear_correction");

  // the cloth dimensions
  istr >> token >> nx >> ny; // (units == meters)
  assert (token == "m");
  assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  Vec3f a,b,c,d;
  istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  istr >> token >> fabric_weight; assert (token == "fabric_weight");
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);

  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) {
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
    }
  }

  // the fixed particles
  while (istr >> token) {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }
	int i,j,k,index;
	float dx,dy;
	Vec3f pos,vel,tmpPos;
	int neighborNum;
	int* neighbors;

	dx = float(1.0/(nx-1));
	dy = float(1.0/(ny-1));
	// cout << "dx=" <<dx << " dy="<< dy << endl;
	  // ASSIGNMENT:
	// move the vertices of the mesh
	for (j=0;j<ny-1;j++){
		for(i=0;i<nx;i++){
			particles[i+nx*j].setNeighborNum(4);
			particles[i+nx*j].setBoundaryPos(INNER);
		}
	}

	for(i=1;i<nx-1;i++)// bottom row
		{particles[i].setNeighborNum(3);particles[i].setBoundaryPos(BOTTOM);}
	for(i=1;i<nx-1;i++)//top most row
		{particles[nx*(ny-1)+i].setNeighborNum(3);particles[nx*(ny-1)+i].setBoundaryPos(TOP);}
	for(j=1;j<ny-1;j++)// left most column
		{particles[nx*j].setNeighborNum(3);particles[nx*j].setBoundaryPos(LEFT);}
	for(j=2;j<ny;j++) // right most column
		{particles[nx*j-1].setNeighborNum(3);particles[nx*j-1].setBoundaryPos(RIGHT);}

	particles[0].setNeighborNum(2);
	particles[0].setBoundaryPos(LB);
	particles[nx-1].setNeighborNum(2);
	particles[nx-1].setBoundaryPos(RB);
	particles[nx*ny-nx].setNeighborNum(2);
	particles[nx*ny-nx].setBoundaryPos(LT);
	particles[nx*ny-1].setNeighborNum(2);
	particles[nx*ny-1].setBoundaryPos(RT);

	for (j=0;j<ny;j++){
		for(i=0;i<nx;i++){
	index=i+nx*j;
	neighborNum = particles[index].getNeighborNum();
	//for(k=0;k<neighborNum;k++){

		switch(particles[index].getBoundaryPos()){
		case LB:
			particles[index].setNeighborId(0,-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,-1);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,-1);
			particles[index].setNeighborId(5,-1);
			particles[index].setNeighborId(6,-1);
			particles[index].setNeighborId(7,index+nx+1);

			break;
		case RB:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,-1);
			particles[index].setNeighborId(2,-1);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,-1);
			particles[index].setNeighborId(5,-1);
			particles[index].setNeighborId(6,index+nx-1);
			particles[index].setNeighborId(7,-1);
			break;
		case LT:
			particles[index].setNeighborId(0,-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,-1);
			particles[index].setNeighborId(4,-1);
			particles[index].setNeighborId(5,index-nx+1);
			particles[index].setNeighborId(6,-1);
			particles[index].setNeighborId(7,-1);
			break;
		case RT:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,-1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,-1);
			particles[index].setNeighborId(4,index-nx-1);
			particles[index].setNeighborId(5,-1);
			particles[index].setNeighborId(6,-1);
			particles[index].setNeighborId(7,-1);
			break;
		case BOTTOM:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,-1);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,-1);
			particles[index].setNeighborId(5,-1);
			particles[index].setNeighborId(6,index+nx-1);
			particles[index].setNeighborId(7,index+nx+1);
			break;
		case TOP:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,-1);
			particles[index].setNeighborId(4,index-nx-1);
			particles[index].setNeighborId(5,index-nx+1);
			particles[index].setNeighborId(6,-1);
			particles[index].setNeighborId(7,-1);
			break;
		case LEFT:
			particles[index].setNeighborId(0,-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,-1);
			particles[index].setNeighborId(5,index-nx+1);
			particles[index].setNeighborId(6,-1);
			particles[index].setNeighborId(7,index+nx+1);
			break;
		case RIGHT:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,-1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,index-nx-1);
			particles[index].setNeighborId(5,-1);
			particles[index].setNeighborId(6,index+nx-1);
			particles[index].setNeighborId(7,-1);
			break;
		case INNER:
			particles[index].setNeighborId(0,index-1);
			particles[index].setNeighborId(1,index+1);
			particles[index].setNeighborId(2,index-nx);
			particles[index].setNeighborId(3,index+nx);
			particles[index].setNeighborId(4,index-nx-1);
			particles[index].setNeighborId(5,index-nx+1);
			particles[index].setNeighborId(6,index+nx-1);
			particles[index].setNeighborId(7,index+nx+1);

			break;
		}
	//cout << "neighborNum " <<neighborNum << endl;
	 //	cout << "p("<< i << "," <<j<<"):"<< i+nx*j <<"=" << particles[i+nx*j].getNeighborId(0)<< " ";
	 //	cout << particles[i+nx*j].getNeighborId(1) << " "<<particles[i+nx*j].getNeighborId(2) << " ";
	 //	cout << particles[i+nx*j].getNeighborId(3) << " "<< particles[i+nx*j].getNeighborId(4) << " ";
	 //	cout << particles[i+nx*j].getNeighborId(5) << " " << particles[i+nx*j].getNeighborId(6) << " ";
	 //	cout << particles[i+nx*j].getNeighborId(7) << endl;

	// 	cout << "pos" << particles[i+nx*j].getOriginalPosition() << endl;

		}
	//	cout << endl;
	}
  computeBoundingBox();
}

// ============================================================================

void Cloth::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
}

// ============================================================================
// ============================================================================
// ============================================================================

void Cloth::Paint() const {
  
  // ==========================================================================
  // render the bounding box
  // ==========================================================================
  if (args->bounding_box) 
    box.Paint();

  // ==========================================================================
  // render the particles
  // ==========================================================================

  if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
	const ClothParticle &p = getParticle(i,j);
	const Vec3f &pos = p.getPosition();
	bool fixed = p.isFixed();
	if (fixed) 
	  glColor3f(0,1,0);
	else
	  glColor3f(0,0,0);	
	glVertex3f(pos.x(),pos.y(),pos.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // ==========================================================================
  // render the velocity at each particle
  // ==========================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // ==========================================================================
  // render the force at each particle
  // ==========================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
 

        // ASSIGNMENT: 
        // visualizate the forces acting on each particle


      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // ==========================================================================
  // render the cloth surface
  // ==========================================================================

  if (args->surface) {
    glColor3f(1,1,1);
    glBegin(GL_QUADS);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
	const Vec3f &a = getParticle(i,j).getPosition();
	const Vec3f &b = getParticle(i,j+1).getPosition();
	const Vec3f &c = getParticle(i+1,j+1).getPosition();
	const Vec3f &d = getParticle(i+1,j).getPosition();
	insertNormal(a,b,c,d);
	glVertex3f(a.x(),a.y(),a.z());
	glVertex3f(b.x(),b.y(),b.z());
	glVertex3f(c.x(),c.y(),c.z());
	glVertex3f(d.x(),d.y(),d.z());
      }
    }
    glEnd();
  }

  // ==========================================================================
  // render the wireframe cloth (the structural and shear springs)
  // ==========================================================================

  if (args->wireframe) {
    glDisable(GL_LIGHTING);
    glLineWidth(1);
    glBegin(GL_LINES);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        const Vec3f &a_o = getParticle(i,j).getOriginalPosition();
        const Vec3f &b_o = getParticle(i,j+1).getOriginalPosition();
        const Vec3f &c_o = getParticle(i+1,j+1).getOriginalPosition();
        const Vec3f &d_o = getParticle(i+1,j).getOriginalPosition();        
        const Vec3f &a = getParticle(i,j).getPosition();
        const Vec3f &b = getParticle(i,j+1).getPosition();
        const Vec3f &c = getParticle(i+1,j+1).getPosition();
        const Vec3f &d = getParticle(i+1,j).getPosition();        
        DrawSpring(a_o,b_o,a,b, provot_structural_correction);
        DrawSpring(a_o,c_o,a,c, provot_shear_correction);
        DrawSpring(a_o,d_o,a,d, provot_structural_correction);
        DrawSpring(b_o,c_o,b,c, provot_structural_correction);
        DrawSpring(b_o,d_o,b,d, provot_shear_correction);
        DrawSpring(c_o,d_o,c,d, provot_structural_correction);
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  }
}



void Cloth::computeDistancetoNeighbors(int id){
	int neighborId;
	Vec3f diff;
	float distance,lmax;
	lmax = 2.85;

	//cout << "this particles id" << id << endl;
	for(int i=0;i<8;i++){
		neighborId=particles[id].neighbors[i];
		if(neighborId != -1){
		  diff=particles[id].position-particles[neighborId].position;
		  distance= diff.Length();
		 // cout<< "neighbor"<< neighborId << ": " << particles[neighborId].position << " distance:" <<distance << endl;
		  if(distance>lmax)
			  particles[id].setFixed(true);
		}
		}

  }
// ============================================================================
// ============================================================================

void Cloth::Animate() {
	int i,j,k,index;
	float dx,dy;
	Vec3f pos,vel,tmpPos;
	int neighborNum;
	int* neighbors;

	dx = float(1.0/(nx-1));
	dy = float(1.0/(ny-1));

Vec3f forces[8],damping_forces[8];
Vec3f totalForce;
float length,xz,xzNew,x,y,z;
float alpha,beta;
float slopeXZ,slopeXZY;
float lmax;
float particleMass;
float timestep=0.01;
lmax = 5*dx;
//this->getParticle(i,j).;
Vec3f gravity,zero;
zero.Set(0.0,0.0,0.0);

for (j=0;j<ny;j++){
	for(i=0;i<nx;i++){
		particleMass =  getParticle(i,j).getMass();
		for (k=0;k<8;k++){
			getParticle(i,j).setForce(k,zero);
		    forces[k].Set(0.0,0.0,0.0);
			damping_forces[k].Set(0.0,0.0,0.0);
		}

		gravity.Set(0.0,-9.81,0.0);
		gravity.Set(0.0,-9.81*particleMass,0.0);
Vec3f diff,dist,e,f1;
float fds,velDiff,v1,v2;
int i_neighbor,j_neighbor,neighborId,id;

if(getParticle(i,j).getBoundaryPos()!=LB && getParticle(i,j).getBoundaryPos()!=LT && getParticle(i,j).getBoundaryPos()!=LEFT)
{
	i_neighbor=i-1;
	j_neighbor=j;
	neighborId=L_NEIGHBOR;
	id=R_NEIGHBOR;
}
if(getParticle(i,j).getBoundaryPos()!=RB && getParticle(i,j).getBoundaryPos()!=RT && getParticle(i,j).getBoundaryPos()!=RIGHT)
{
	i_neighbor=i+1;
	j_neighbor=j;
	neighborId=R_NEIGHBOR;
	id=L_NEIGHBOR;
}
if(getParticle(i,j).getBoundaryPos()!=LB && getParticle(i,j).getBoundaryPos()!=RB && getParticle(i,j).getBoundaryPos()!=BOTTOM)
{
	i_neighbor=i;
	j_neighbor=j-1;
	neighborId=B_NEIGHBOR;
	id=T_NEIGHBOR;
}
if(getParticle(i,j).getBoundaryPos()!=LT && getParticle(i,j).getBoundaryPos()!=RT && getParticle(i,j).getBoundaryPos()!=TOP)
{
	i_neighbor=i;
	j_neighbor=j+1;
	neighborId=T_NEIGHBOR;
	id=B_NEIGHBOR;
}
//getParticle(0,0).setFixed(true);
//getParticle(0,1).setFixed(true);
//getParticle(0,2).setFixed(true);
//getParticle(1,0).setFixed(true);
//getParticle(1,1).setFixed(true);
//getParticle(1,2).setFixed(true);
//getParticle(2,0).setFixed(true);
//getParticle(2,1).setFixed(true);

//getParticle(2,2).setFixed(true);

//getParticle(2,2).printNeighbors();
computeDistancetoNeighbors(i+nx*j);


diff = getParticle(i,j).getPosition()-getParticle(i_neighbor,j_neighbor).getPosition();

v2=getParticle(i_neighbor,j_neighbor).getVelocity().Dot3(e);

e = diff;
e.Normalize();
v1=getParticle(i,j).getVelocity().Dot3(e);
velDiff=abs(v1-v2);
fds = -k_structural*(abs(diff.Length()-dx));//-damping*velDiff;
//cout << "diff"<<diff.Length()<<" dx "<<dx<<"fds"<<fds<< endl;
f1= fds*e;

getParticle(i,j).setForce(neighborId,f1);
getParticle(i_neighbor,j_neighbor).setForce(id,-f1);


		for (k=0;k<8;k++){
			totalForce=totalForce+getParticle(i,j).getForce(k);
			//cout << "force (" << k << ")=" <<getParticle(i,j).getForce(k) << "totla="<<  totalForce<<endl;

		}
//cout << "TOTAL FORCE"<<totalForce<<endl;
	//	totalForce.sety((gravity.y()*particleMass));
			totalForce.sety(1.0);
			getParticle(i,j).setAcceleration(totalForce)  ;
			pos = getParticle(i,j).getPosition();
			vel = getParticle(i,j).getVelocity();
			vel = vel+timestep*getParticle(i,j).getAcceleration();
			pos = pos+timestep*getParticle(i,j).getVelocity();

			//length = sqrt( sqrt(pos.x()*pos.x()+pos.z()*pos.z())*sqrt(pos.x()*pos.x()+pos.z()*pos.z())+pos.y()*pos.y());
			x= abs(pos.x()-getParticle(i,j).getPosition().x());
			z= abs(pos.z()-getParticle(i,j).getPosition().z());

			if(getParticle(i,j).isFixed())
				tmpPos=getParticle(i,j).getOriginalPosition()-pos;
			if(getParticle(i,j).isFixed())
				tmpPos=getParticle(i,j).getPosition()-pos;

			length=tmpPos.Length();

			xz = sqrt(pos.x()*pos.x()+pos.z()*pos.z()+pos.y()*pos.y());
		//	cout <<"Lebgth("<<i<<","<<j<< ")="<<length << " " << endl;

//////

		//	particles[i+nx*j].setFixed(false);
			if(length>dx)
						{
					//	cout << "lmax=" <<lmax << " l_spring" << length << endl;
//						slopeXZ= (abs(pos.z()-particles[i+nx*j].getPosition().z()))/(abs(pos.x()-particles[i+nx*j].getPosition().x()));
//						alpha = atan(slopeXZ);
//						slopeXZY = xz/(abs(pos.y()-particles[i+nx*j].getPosition().y()));
//						beta = atan(slopeXZY);
//						pos.sety(lmax*sin(beta));
//						xzNew = lmax*cos(beta);
//
//						pos.setx(sin(alpha)*xzNew);
//						pos.setz(cos(alpha)*xzNew);
//						particles[nx*j+i].setPosition(pos);
				//cout << "uzuun"<<endl;
			 	 	getParticle(i,j).setFixed(true);
			 	 	getParticle(i,j).setOriginalPosition(pos);
							}
			if(!getParticle(i,j).isFixed())
				{
					getParticle(i,j).setPosition(pos);
					getParticle(i,j).setVelocity(vel);
				}
			}

		//cout << endl;
		//	cout << particles[i+nx*j].getAcceleration();
		}
	// cout << endl;

	/* for(i=0;i<nx;i++){
		pos = particles[nx*ny-1-i].getPosition();
		vel = particles[nx*ny-1-i].getVelocity();

		pos.setz(pos.z()+0.001*vel.z());
		vel.setz(vel.z()+0.00001*(0.81));
		//particles[i].setFixed(true);
		particles[nx*ny-1-i].setVelocity(vel);
		particles[nx*ny-1-i].setPosition(pos);
	 }
*/

}

// ============================================================================
// ============================================================================
// some helper drawing functions
// ============================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ============================================================================
// ============================================================================
